perm filename HANDSE.SAI[SYS,HE] blob
sn#004215 filedate 1972-09-20 generic text, type T, neo UTF8
00100 COMMENT ⊗ VALID 00004 PAGES
00200 RECORD PAGE DESCRIPTION
00300 00001 00001
00400 00002 00002 ⊃ Needs the following to be defined: ⊃, CRLF
00500 00005 00003 SIMPLE PROCEDURE NORML(REAL XIB,YIB,XIC,YIC,XID,YID REAL ARRAY DIRE)
00600 00008 00004 PROCEDURE POSE_HAND(REAL XH,YH,ZH,DIRX,DIRY REFERENCE INTEGER HNDFLG)
00700 00010 ENDMK
00800 ⊗;
00100 ⊃ Needs the following to be defined: ⊃, CRLF;
00200
00300 PROCEDURE IMAGECOR(REAL XTC,YTC,ZTC; REFERENCE REAL XIC,YIC);
00400 ⊃ Transform table coords to image coords. use COL;
00500 BEGIN "IMAGECOR"
00600 INTEGER I,J,K;
00700 REAL AC;
00800 REAL ARRAY V,U[1:3];
00900 AC←CAMERA_MODEL[4,3];
01000 V[1]←(CAMERA_MODEL[4,1]*ZTC-AC*XTC)/(ZTC-AC);
01100 V[2]←(CAMERA_MODEL[4,2]*ZTC-AC*YTC)/(ZTC-AC);
01200 V[3]←1;
01300 FOR I←1 STEP 1 UNTIL 3 DO BEGIN
01400 AC←0;
01500 FOR J←1 STEP 1 UNTIL 3 DO AC←AC+CAMERA_MODEL[I,J]*V[J];
01600 U[I]←AC; END;
01700 XIC←U[1]/U[3]; YIC←U[2]/U[3];
01800 END "IMAGECOR";
01900
02000 PROCEDURE TABLECOR(REAL XIC,YIC; REFERENCE REAL XTC,YTC);
02100 ⊃ Transform image coords to tabletop coords. use ICOL;
02200 BEGIN "TABLECOR"
02300 INTEGER I,J,K;
02400 REAL AC;
02500 REAL ARRAY V,U[1:3];
02600 V[1]←XIC; V[2]←YIC; V[3]←1;
02700 FOR I←1 STEP 1 UNTIL 3 DO BEGIN
02800 AC←0;
02900 FOR J←1 STEP 1 UNTIL 3 DO AC←AC+CAMERA_MODEL[5+I,J]*V[J];
03000 U[I]←AC; END;
03100 XTC←U[1]/U[3]; YTC←U[2]/U[3];
03200 END "TABLECOR";
03300
03400 SIMPLE PROCEDURE HOME_ARM;
03500 BEGIN "HOME_ARM"
03600 MESS←ISSUE(5,"DRV","HAND",MESSAGE START_TRAJECTORY("MASLUL",FALSE));
03700 MESS←ISSUE(5,"DRV","HAND",MESSAGE OPEN_HAND(3));
03800 MESS←ISSUE(5,"DRV","HAND",MESSAGE PARK_ARM);
03900 MESS←ISSUE(5,"DRV","HAND",MESSAGE CLOSE_TRAJECTORY);
04000 MESS←ISSUE(7,"DRV","HAND",MESSAGE DO_IT("MASLUL"));
04100 IF ARM_STATUS≠0
04200 THEN OUTSTR("HOME_ARM: ARM FAILED (ARM_STATUS="&CVOS(ARM_STATUS)&")"CRLF);
04300 END "HOME_ARM";
00100 SIMPLE PROCEDURE NORML(REAL XIB,YIB,XIC,YIC,XID,YID; REAL ARRAY DIRE);
00200 BEGIN "NORML"
00300 REAL D;
00400 FORTRAN REAL PROCEDURE SQRT(REAL X);
00500
00600 D←SQRT((XIB-XIC)↑2+(YIB-YIC)↑2);
00700 DIRE[1]←(XIB-XIC)/D; DIRE[2]←(YIB-YIC)/D;
00800 D←SQRT((XID-XIC)↑2+(YID-YIC)↑2);
00900 DIRE[3]←(XID-XIC)/D; DIRE[4]←(YID-YIC)/D;
01000 IF (DIRE[1]*DIRE[4]-DIRE[2]*DIRE[3])<0
01100 THEN BEGIN DIRE[1]↔DIRE[3]; DIRE[2]↔DIRE[4]; END;
01200 DIRE[5]←XIC; DIRE[6]←YIC; DIRE[7]←0; DIRE[8]←0;
01300 END "NORML";
01400
01500 SIMPLE PROCEDURE ARMTRAN(REAL DX,DY,DZ; REFERENCE REAL XTC,YTC,ZTC);
01600 BEGIN "ARMTRAN"
01700 XTC←ARM_LINK[6,1,1]*DX+ARM_LINK[6,1,2]*DY+ARM_LINK[6,1,3]*DZ+ARM_LINK[6,1,4];
01800 YTC←ARM_LINK[6,2,1]*DX+ARM_LINK[6,2,2]*DY+ARM_LINK[6,2,3]*DZ+ARM_LINK[6,2,4];
01900 ZTC←ARM_LINK[6,3,1]*DX+ARM_LINK[6,3,2]*DY+ARM_LINK[6,3,3]*DZ+ARM_LINK[6,3,4];
02000 END "ARMTRAN";
02100
02200 SIMPLE PROCEDURE CORN_HAND(REAL DX,DY,DZ; REAL ARRAY DIRD);
02300 BEGIN "CORN_HAND"
02400 REAL XTC,YTC,ZTC,XIB,YIB,XIC,YIC,XID,YID;
02500 ARMTRAN(DX,DY,DZ,XTC,YTC,ZTC);
02600 IMAGECOR(XTC,YTC,ZTC,XIC,YIC);
02700 ARMTRAN(-DX,DY,DZ,XTC,YTC,ZTC);
02800 IMAGECOR(XTC,YTC,ZTC,XIB,YIB);
02900 ARMTRAN(DX,DY,DZ-.25,XTC,YTC,ZTC);
03000 IMAGECOR(XTC,YTC,ZTC,XID,YID);
03100 NORML(XIB,YIB,XIC,YIC,XID,YID,DIRD);
03200 END "CORN_HAND";
03300
03400 SIMPLE REAL PROCEDURE GRASP_CAL;
03500 BEGIN REAL GCOR;
03600 ARM_EXECUTE←S_O_T←TRUE;
03700 MESS←ISSUE(7,"DRV","HAND",MESSAGE OPEN_HAND(-1));
03800 IF (ARM_STATUS=6)∨(ARM_STATUS='16)
03900 THEN BEGIN GCOR←GRASP;
04000 OUTSTR("GRASP_CAL: GCOR="&CVG(GCOR)CRLF);
04100 MESS←ISSUE(7,"DRV","HAND",MESSAGE DO_PROCEED); END
04200 ELSE OUTSTR("GRASP_CAL-FAILED (ARM_STATUS="&CVOS(ARM_STATUS)&")"CRLF);
04300 S_O_T←FALSE;
04350 ARM_EXECUTE←TRUE;
04400 MESS←ISSUE(7,"DRV","HAND",MESSAGE OPEN_HAND(3));
04500 RETURN(GCOR);
04600 END "GRASP_CAL";
00100 PROCEDURE POSE_HAND(REAL XH,YH,ZH,DIRX,DIRY; REFERENCE INTEGER HNDFLG);
00200 BEGIN "POSE_HAND"
00300 REAL ARRAY TRANS[1:4,1:4];
00400
00500 HNDFLG←0;
00600 TRANS[4,1]←TRANS[4,2]←TRANS[4,3]←0; TRANS[4,4]←1;
00700 TRANS[1,3]←TRANS[2,3]←0; TRANS[3,3]←-1;
00800 TRANS[3,1]←TRANS[3,2]←0;
00900 TRANS[1,4]←XH; TRANS[2,4]←YH; TRANS[3,4]←ZH;
01000 TRANS[2,1]←TRANS[1,2]←DIRX;
01100 TRANS[1,1]←-(TRANS[2,2]←DIRY);
01200 MESS←ISSUE(5,"DRV","HAND",MESSAGE START_TRAJECTORY("MASLUL",FALSE));
01300 MESS←ISSUE(7,"DRV","HAND",MESSAGE MOVE_ARM(TRANS,ARM_PLAN));
01400 IF ARM_PLAN
01500 THEN MESS←ISSUE(5,"DRV","HAND",MESSAGE CLOSE_TRAJECTORY)
01600 ELSE BEGIN OUTSTR("POSE_HAND: THEN ARM CAN'T REACH THIS LOCATION"CRLF);
01700 HNDFLG←1; RETURN; END;
01800 MESS←ISSUE(7,"DRV","HAND",MESSAGE DO_IT("MASLUL"));
01900 IF ARM_STATUS≠0 THEN BEGIN
02000 OUTSTR("POSE_HAND: ARM FAILED (ARM_STATUS"&CVOS(ARM_STATUS)&")"CRLF);
02100 HNDFLG←2; RETURN; END;
02200 END "POSE_HAND";